#include "pid_ctrl.h"
#define DRV_OUTPUT_MAX 10.26F
#define BARTY_OUT_MAX 24.f/1.732f
/**
  * @brief  pid calculation
  * @param  pid    the pid struct containing parameters
  * @param  ref    reference value
  * @param  cur    current value
  * @return pid calculation result
  * @note pid->k_deadband is invaild in this process
  */
float pid_calc(pid_struct_t *pid, float ref, float cur){
  float output;
  pid->err[1] = pid->err[0];
  pid->err[0] = ref - cur;
  
  pid->p_out  = pid->kp * pid->err[0];
  pid->i_out += pid->ki * pid->err[0];
  pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);
  
  output = pid->p_out + pid->i_out + pid->d_out;
  LIMIT_MIN_MAX(output, -pid->out_max, pid->out_max);
  return output;
}

const pid_struct_t _s_PID={
   .kp = 0.00026411f,
   .ki = 0.0041759f,
   .kd=0.0f,
   .i_max=DRV_OUTPUT_MAX/50,
   .out_max=DRV_OUTPUT_MAX,
   .k_deadband=0.0f 
};

const pid_struct_t _cd_PID={
   .kp = 0.294f,
   .ki = 0.f,
   .kd=0.0f,
   .i_max=BARTY_OUT_MAX/50,
   .out_max=BARTY_OUT_MAX,
   .k_deadband=0.0f 
};

const pid_struct_t _cq_PID={
   .kp = 0.294f,
   .ki = 0.f,
   .kd=0.0f,
   .i_max=BARTY_OUT_MAX/50,
   .out_max=BARTY_OUT_MAX,
   .k_deadband=0.0f 
};

